#include "S_ENCODER.h"

static int32_t encoder_count[4] = {0};

static GPIO_TypeDef *encoder_port[4] = {
	M1AI_GPIO_Port, M1BI_GPIO_Port,
	M2AI_GPIO_Port, M2BI_GPIO_Port};
static uint16_t encoder_pin[4] = {
	M1AI_Pin, M1BI_Pin,
	M2AI_Pin, M2BI_Pin};

void ENCODER_Callback(uint16_t GPIO_Pin)
{
	for (uint8_t i = 0; i < 4; i++)
		if (GPIO_Pin == encoder_pin[i])
		{
			if (i % 2 == 0) // A相
				encoder_count[i / 2] += 1 - 2 * (HAL_GPIO_ReadPin(encoder_port[i], encoder_pin[i]) & HAL_GPIO_ReadPin(encoder_port[i + 1], encoder_pin[i + 1]));
			else // B相
				encoder_count[i / 2] -= 1 - 2 * (HAL_GPIO_ReadPin(encoder_port[i], encoder_pin[i]) & HAL_GPIO_ReadPin(encoder_port[i + 1], encoder_pin[i + 1]));
			break;
		}
}

int32_t GetEncoderVal(uint8_t encodernum)
{
	if (encodernum > 3)
		return 0;
	int32_t val = encoder_count[encodernum];
	encoder_count[encodernum] = 0;
	return val;
}